/*
 * Copyright (c) 2017, AEye
 * All rights reserved.
 */

#include <utility>

#include "spdlog/spdlog.h"

#include "HawkeyeFrame.hpp"
#include "PcapPacket.hpp"
#include "SensorReader.hpp"
#include "Utils.hpp"

using namespace aeye;

SensorReader::SensorReader(
    std::string ipAddress, uint16_t cloudPort,
    uint16_t diagnosticPort, uint16_t hawkeyePort,
    std::string name, bool listenerMode
) :
    _ipAddress(std::move(ipAddress)), _name(std::move(name)),
    cloudReader(_ipAddress, cloudPort, listenerMode),
    hawkeyeReader(_ipAddress, hawkeyePort, listenerMode),
    diagnosticReader(_ipAddress, diagnosticPort),
    _isRunning(false), isHawkeyeRunning(false), isDiagnosticRunning(false)
    { }

SensorReader::~SensorReader(){
    stop();
}

const std::string &SensorReader::ipAddress() const {
    return _ipAddress;
}

const std::string &SensorReader::name() const {
    return _name;
}

void SensorReader::setCloudCallback(const std::function<void(PointCloudFrame&&)>& processCallback){
    _pointCloudCallback = processCallback;
}

void SensorReader::setDiagnosticCallback(const std::function<void(aeye::DiagnosticData&&)>& processCallback){
  _diagnosticCallback = processCallback;
}

void SensorReader::setHawkeyeCallback(const std::function<void(HawkeyeFrame&&)>& processCallback) {
  _hawkeyeCallback = processCallback;
}

/*
 *  thread function
 */
void SensorReader::start(){
  cloudReader.start();
  cloudPublisher  = std::thread(&SocketCloudReader::run, &cloudReader);
  _isRunning      = true;
  cloudSubscriber = std::thread(&SensorReader::collectCloud, this);
}

void SensorReader::collectCloud() {
  set_thread_name(AEYE_LIT("CollectCloud"));
  while (_isRunning) {
    if (_pointCloudCallback) {
      _pointCloudCallback(cloudReader.pop());
    }
    else {
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
  }
}

void SensorReader::startHawkeye() {
  if (isHawkeyeRunning) {
    spdlog::warn("Attempted to start already running HawkEye reader.");
    return;
  }
  hawkeyeReader.start();
  hawkeyePublisher  = std::thread(&SocketHawkeyeReader::run, &hawkeyeReader);
  isHawkeyeRunning  = true;
  hawkeyeSubscriber = std::thread(&SensorReader::collectHawkeyeBuffer, this);
}

void SensorReader::collectHawkeyeBuffer() {
  set_thread_name(AEYE_LIT("CollectHawkEye"));
  while (isHawkeyeRunning) {
    if (_hawkeyeCallback) {
      _hawkeyeCallback(hawkeyeReader.pop());
    } else {
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
  }
}

void SensorReader::stopHawkeye() {
  isHawkeyeRunning = false;
  hawkeyeReader.stop();
  if (hawkeyePublisher.joinable()) hawkeyePublisher.join();
  if (hawkeyeSubscriber.joinable()) hawkeyeSubscriber.join();
}

void SensorReader::startDiagnostic() {
  if (isDiagnosticRunning) {
    spdlog::warn("Attempted to start already running Diagnostic reader.");
    return;
  }
  diagnosticReader.start();
  diagnosticPublisher  = std::thread(&SocketDiagnosticReader::run, &diagnosticReader);
  isDiagnosticRunning  = true;
  diagnosticSubscriber = std::thread(&SensorReader::collectDiagnostic, this);
}

void SensorReader::collectDiagnostic() {
  set_thread_name(AEYE_LIT("CollectDiag"));
  while (isDiagnosticRunning) {
    if (_diagnosticCallback) {
      _diagnosticCallback(diagnosticReader.pop());
    }
    else {
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
  }
}

void SensorReader::stopDiagnostic() {
  isDiagnosticRunning = false;
  diagnosticReader.stop();
  if (diagnosticPublisher.joinable()) diagnosticPublisher.join();
  if (diagnosticSubscriber.joinable()) diagnosticSubscriber.join();
}

void SensorReader::stop() {
    if (isHawkeyeRunning)
      stopHawkeye();
    if (isDiagnosticRunning)
      stopDiagnostic();
    cloudReader.stop();
    _isRunning = false;
    if(cloudSubscriber.joinable()) cloudSubscriber.join();
    if(cloudPublisher.joinable()) cloudPublisher.join();
}

void SensorReader::processRangingPcapPacket(PcapPacket& packet) {
  cloudReader.pushPcapPacket(std::move(packet.getData()));
}

void SensorReader::processDiagnosticPcapPacket(PcapPacket& packet){
  diagnosticReader.pushPcapPacket(std::move(packet.getData()));
}

void SensorReader::setPcapMode(bool value) {
  cloudReader.setPcapMode(value);
  diagnosticReader.setPcapMode(value);
}

void SensorReader::resetSocketReaders() {
  cloudReader.resetReader();
}